#ifndef __IMPLEMENT_PLANNER_H__
#define __IMPLEMENT_PLANNER_H__

#include <unordered_map>
#include <vector>

#include <geometry_msgs/Pose.h>

namespace global_path_planner {
namespace implement {

class ImplementPlanner{
public:
    explicit ImplementPlanner();
    virtual ~ImplementPlanner();

    // 记录手动生成路径
    void RecordManualPath();

    // 获取手动生成路径
    std::vector<geometry_msgs::Pose> GetImplementPath();

    // geometry_msgs::Pose -> pair<float, float>转换
    std::vector<std::pair<float, float>> 
        PoseTransferPairs(std::vector<geometry_msgs::Pose>& path_poses);

    // 清空缓存数据
    void ClearPath();

    // 设置记录是否进行
    void SetThreadBusing(bool thread_busing);

    // 获取当前线程状态
    bool GetThreadBusing() const{
        return __thread_Busying;
    }

private:
    // 获取机器人位姿
    void GetRobotPose(bool & state, std::unordered_map<std::string, float>& pose);

    // 计算两点之间的距离
    float DistForP2P(std::unordered_map<std::string, float>& pre, 
                    std::unordered_map<std::string, float>& cur);

private:
    // 线程工作状态
    bool __thread_Busying;
    // 生成记忆路径
    std::vector<std::unordered_map<std::string, float>> __way_Points;
    // 两点之间的欧式距离阈值
    float __interval_Distance;
};

}  // namespace implement
}  // namespace global_path_planner

#endif  // __IMPLEMENT_PLANNER_H__